
/* 第一个运动学模型 */
/* 加上功率平衡 */

// 车轮半径参数 12cm
// 轮距 423mm
// 轴距 538mm
// 更改算法  为 Speed&Power 算法

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "modbus/modbus.h"

#include "rclcpp/rclcpp.hpp"
#include "basic_sprayer_interfaces/msg/four_drivers.hpp" // 四个驱动
#include "basic_sprayer_interfaces/msg/four_wheel.hpp"   // 四轮速度
#include "basic_sprayer_interfaces/msg/adc.hpp"          // adc的Message
#include "basic_sprayer_interfaces/msg/human_cmd.hpp"    // 控制信号
#include "basic_sprayer_interfaces/msg/pid.hpp"          // 四轮速度
#include "basic_sprayer_interfaces/msg/debug_power.hpp"

#include <math.h>
#include "pid.h"

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif // !M_PI

/* 将速度控制在100Hz  也就是每采样一次 转角运算一次速度 */

class FourValue
{
public:
    FourValue(float V_fl, float V_fr, float V_rl, float V_rr)
    {
        this->V_fr = V_fr;
        this->V_fl = V_fl;
        this->V_rl = V_rl;
        this->V_rr = V_rr;
    }
    float V_fr, V_fl, V_rl, V_rr;
};

class kineControler
{
public:
    /* 调试 */
    float debug_delta[4];
    float debug_spd[4];

    kineControler()
    {
        pi_p = 0.06;
        pi_d = 0.0005;

        wheelTrack = 0.423;
        wheelBase = 0.538;
        wheelRadius = 0.12;

        // speed_2_rpm = 60 / (0.12 * 2 * M_PI);
        // 1920 pulse for 1
        // double rpm_2_speed = 1.0 * 6 / 1920 / 60 * 2 * M_PI * 0.46;

        speed_2_rpm = 60 / (0.12 * 2 * M_PI);
        rpm_2_speed = 1.0 / speed_2_rpm;

        dot_angle_front = 0;
        dot_angle_rear = 0;
        {
            speed_cl = pid control(0.0008, 0.00004, 0, 0.1, 0.1); // 速度
        }
        {
            fb_bridge = pid control(0.0008, 0.00004, 0, 0.0, 0.0); // 前后桥
        }
        {
            lr_brideg = pid control(0.0008, 0.00004, 0, 0.0, 0.0); // 左右轮
        }
    }

    void set_pd(float p, float d)
    {
        pi_p = p;
        pi_d = d;
    }
    void set_power_blance_pi(float p, float i)
    {
        for (int j = 0; j < 4; j++)
        {
            power_controler[j].set_p(p);
            power_controler[j].set_i(i);
        }
    }

    FourValue run(float target_speed, float target_deg);
    float power_restrain();                           // 功率抑制
    float error_calc(float target_w, float driver_w); // 计算误差
    void re_adjust(pid *power_controler);
    void clear();

    void setStatus(float angle_front_deg, float angle_rear_deg)
    {
        /* 可以计算d 做一个低通滤波 */
        // dot_angle_front = 0.5 * dot_angle_front + 0.5 * (angle_front - angle_front_deg) * 100;
        // dot_angle_rear = 0.5 * dot_angle_rear + 0.5 * (angle_rear - angle_rear_deg) * 100;

        dot_angle_front = (angle_front - angle_front_deg) * 100;
        dot_angle_rear = (angle_rear - angle_rear_deg) * 100;

        angle_front = angle_front_deg;
        angle_rear = angle_rear_deg;
    }

    void set_power(float power_fl, float power_fr, float power_rl, float power_rr)
    {
        power[0] = power_fl;
        power[1] = power_fr;
        power[2] = power_rl;
        power[3] = power_rr;
        power_avg = (power_fl + power_fr + power_rl + power_rr) / 4;
    }

    void set_current(float current_fl, float current_fr, float current_rl, float current_rr)
    {
        current[0] = current_fl;
        current[1] = current_fr;
        current[2] = current_rl;
        current[3] = current_rr;
        current_avg = (current_fl + current_fr + current_rl + current_rr) / 4;
    }

    void set_speed(float speed_fl, float speed_fr, float speed_rl, float speed_rr)
    {
        speed[0] = speed_fl;
        speed[1] = speed_fr;
        speed[2] = speed_rl;
        speed[3] = speed_rr;
        speed_avg = (speed_fl + speed_fr + speed_rl + speed_rr) / 4;
    }

private:
    float wheelTrack;  // 轮距
    float wheelBase;   // 轴距
    float wheelRadius; // 车轮半径

    float speed_2_rpm;
    float rpm_2_speed;

    float angle_front; // 前后转角
    float angle_rear;

    float dot_angle_front;
    float dot_angle_rear;

    float pi_p; // p参数
    float pi_d; // d参数

    /* power balance */
    float current[4] = {0};
    float current_goal[4] = {0};
    float current_avg = 0;

    /* 平均速度 */
    float speed[4] = [0];
    float speed_avg[4] = [0];

    float power_avg;     // 平均功率
    float power[4];      // 四个功率
    float power_goal[4]; // 目标功率

    // pid power_controler[4]; // 控制器
    // pid power_dis[2];       // 控制器调整功率
    /* 前后一致 */
    /* 左右一致 */
    pid speed_cl;     // 速度闭环
    pid fb_bridge;    // 前后桥 平均值  努力相等
    pid lr_brideg[2]; // 左右桥

    float power_delat_v[4]; // 速度给定值
};

void kineControler::clear()
{
    // 清除数据
    // for (int i = 0; i < 4; i++)
    // {
    //     power_controler[i].clear();
    // }
    // power_dis[0].clear();
    // power_dis[1].clear();
    speed_cl.clear();
    fb_bridge.clear();
    lr_brideg[0].clear();
    lr_brideg[1].clear();
}

FourValue kineControler::run(float target_speed, float target_deg)
{
    /*前后转向桥无约束*/
    float angele_rad = 0;
    float v_fl, v_fr, v_rl, v_rr;
    float front_error = target_deg - angle_front;
    float rear_error = -target_deg - angle_rear;
    // float front_out = front_error * pi_p * wheelTrack / 2 + dot_angle_front * pi_d;
    // float rear_out = rear_error * pi_p * wheelTrack / 2 + dot_angle_rear * pi_d;

    float front_out = front_error * pi_p * wheelTrack / 2 + dot_angle_front * pi_d;
    float rear_out = rear_error * pi_p * wheelTrack / 2 + dot_angle_rear * pi_d;
    // float rear_out = -front_out;

    // if (fabs(angle_front - angle_rear) > 1) // 角度不为0，可以运算
    if (angle_front * angle_rear < 0) // 不同符号
    {
        // angele_rad = angle_front * M_PI / 180; // 以前轮为准 后轮取反
        angele_rad = (angle_front - angle_rear) / 2 * M_PI / 180; // 以前轮为准 后轮取反
        float rRadius = wheelBase / (2 * sin(angele_rad));
        float wheelTrack_rRadius = wheelTrack / (2 * rRadius);
        v_fl = target_speed * (1 - wheelTrack_rRadius) - front_out;
        v_fr = target_speed * (1 + wheelTrack_rRadius) + front_out;
        v_rl = target_speed * (1 - wheelTrack_rRadius) - rear_out;
        v_rr = target_speed * (1 + wheelTrack_rRadius) + rear_out;
    }
    else
    {
        v_fl = target_speed - front_out;
        v_fr = target_speed + front_out;
        v_rl = target_speed - rear_out;
        v_rr = target_speed + rear_out;
    }
    /* 电流增减  I = dv  通过修改dv修改电流 */

    /* 在电流的均值上进行增减  */

    /* 加一点  减一点   */

    /* 第一步是求平均功率 --> 第二步是做每个功率的计算分配 */
    // float cur = speed_cl.calc_pi(target_speed - speed_avg * rpm_2_speed);
    // /* 功率调整 --> 两个pi控制器   前后桥  一个加 一个减 */
    // float fpi = lr_brideg[0].calc_pi(front_error);
    // float rpi = lr_brideg[1].calc_pi(rear_error);
    float f_avg = (current[0] + current[1]) / 2;
    float b_avg = (current[2] + current[3]) / 2;
    float fb = fb_bridge.calc_pi(f_avg - b_avg);



    current_goal[0] = current_avg + cur;
    current_goal[1] = current_avg + cur;
    current_goal[2] = current_avg + cur;
    current_goal[3] = current_avg + cur;

    /* 两个pi控制器,  */
    current_goal[0] -= fpi;
    current_goal[1] += fpi;
    current_goal[2] -= rpi;
    current_goal[3] += rpi;

    // std::cout<<front_error<<" "<<fpi<<" "<<std::endl;
    /* 调试  期望的power */
    debug_spd[0] = current_goal[0];
    debug_spd[1] = current_goal[1];
    debug_spd[2] = current_goal[2];
    debug_spd[3] = current_goal[3];

    /* 功率 */
    power_delat_v[0] = power_controler[0].calc_pi(current_goal[0] - current[0]);
    power_delat_v[1] = power_controler[1].calc_pi(current_goal[1] - current[1]);
    power_delat_v[2] = power_controler[2].calc_pi(current_goal[2] - current[2]);
    power_delat_v[3] = power_controler[3].calc_pi(current_goal[3] - current[3]);
    // if (target_speed >= 0)
    // {
    v_fl += power_delat_v[0];
    v_fr += power_delat_v[1];
    v_rl += power_delat_v[2];
    v_rr += power_delat_v[3];
    // }
    // else if (target_speed < 0)
    // {
    //     v_fl -= power_delat_v[0];
    //     v_fr -= power_delat_v[1];
    //     v_rl -= power_delat_v[2];
    //     v_rr -= power_delat_v[3];
    // }

    /* 重新分配速度, 使得平均速度不变 */
    /* 转向时更加  */
    float v_avg = (v_fl + v_fr + v_rl + v_rr) / 4; // 速度
    if (fabsf(v_avg) > 0.02)
    {
        float resize = fabsf(target_speed / v_avg);
        v_fl = v_fl * resize;
        v_fr = v_fr * resize;
        v_rl = v_rl * resize;
        v_rr = v_rr * resize;
    }
    printf("target speed %f\n", target_speed);

    debug_delta[0] = power_delat_v[0];
    debug_delta[1] = current_goal[0] - current[0];
    // debug_delta[2] = power_delat_v[2];
    // debug_delta[3] = power_delat_v[3];

    FourValue value(v_fl * speed_2_rpm, v_fr * speed_2_rpm, v_rl * speed_2_rpm, v_rr * speed_2_rpm);
    return value;
}

/*
  // subscriber Liveliness callback event
  subscriber->options().event_callbacks.liveliness_callback =
    [this, &total_number_of_liveliness_events](
    rclcpp::QOSLivelinessChangedInfo & event) -> void
    {
      RCLCPP_INFO(subscriber->get_logger(), "QOSLivelinessChangedInfo callback");
      total_number_of_liveliness_events++;

      // strict checking for expected events
      if (total_number_of_liveliness_events == 1) {
        // publisher came alive
        ASSERT_EQ(1, event.alive_count);
        ASSERT_EQ(0, event.not_alive_count);
        ASSERT_EQ(1, event.alive_count_change);
        ASSERT_EQ(0, event.not_alive_count_change);
      } else if (total_number_of_liveliness_events == 2) {
        // publisher died
        ASSERT_EQ(0, event.alive_count);
        ASSERT_EQ(0, event.not_alive_count);
        ASSERT_EQ(-1, event.alive_count_change);
        ASSERT_EQ(0, event.not_alive_count_change);
      }
    };
*/
using namespace std::chrono_literals;
class kinematicsNode : public rclcpp::Node
{
public:
    kinematicsNode() : Node("kinematicsNode")
    {
        // define qos profile
        // const std::chrono::milliseconds deadline_duration = std::chrono_literals::1s;
        // rclcpp::QoS qos_profile(10);
        // qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_AUTOMATIC);
        // qos_profile.liveliness_lease_duration(1s);
        publisher_ = this->create_publisher<basic_sprayer_interfaces::msg::FourWheel>("driver/fourwheel", 10);
        debug_ = this->create_publisher<basic_sprayer_interfaces::msg::DebugPower>("/hesiwei/debugpower", 10);
        /* 订阅adc */
        subscription_adc = this->create_subscription<basic_sprayer_interfaces::msg::Adc>(
            "/adc", 10, std::bind(&kinematicsNode::callback_adc, this, std::placeholders::_1));
        /* 订阅车身状态 */
        subscription_driver = this->create_subscription<basic_sprayer_interfaces::msg::FourDrivers>(
            "driver/status", 10, std::bind(&kinematicsNode::callback_status, this, std::placeholders::_1));
        /*订阅发布的命令*/
        subscription_cmd = this->create_subscription<basic_sprayer_interfaces::msg::HumanCmd>(
            "/hesiwei/humancmd", 10, std::bind(&kinematicsNode::callback_cmd, this, std::placeholders::_1));

        subscription_pid = this->create_subscription<basic_sprayer_interfaces::msg::Pid>(
            "kinepid", 10, std::bind(&kinematicsNode::pid_callback, this, std::placeholders::_1));
        /*1s 定时*/
        timer_ = this->create_wall_timer(500ms, std::bind(&kinematicsNode::timer_callback, this));
        /* code run */
        timer_100ms = this->create_wall_timer(10ms, std::bind(&kinematicsNode::timer_10ms_callback, this));

        // sub_adc_opt.event_callbacks.liveliness_callback = std::bind(&kinematicsNode::adc_event, this, std::placeholders::_1);
        // sub_adc_opt.event_callbacks.liveliness_callback = [this](rclcpp::QOSLivelinessChangedInfo &info) -> void
        // {
        //     RCLCPP_INFO(this->get_logger(), "adc disconnect alive_count=%d,not_alive_count=%d,alive_count_change=%d,not_alive_count_change=%d", info.alive_count, info.not_alive_count, info.alive_count_change, info.not_alive_count_change);
        // };

        is_start = false;
        speed_target = 0;
        angle_front = 0;
        angle_rear = 0;

        cmd_cnt = 0;
        controler = new kineControler();
        speed_target = 0;
    }

private:
    /*发布消息*/
    rclcpp::Publisher<basic_sprayer_interfaces::msg::FourWheel>::SharedPtr publisher_;

    rclcpp::Subscription<basic_sprayer_interfaces::msg::FourDrivers>::SharedPtr subscription_driver;
    rclcpp::Subscription<basic_sprayer_interfaces::msg::Adc>::SharedPtr subscription_adc;
    rclcpp::Subscription<basic_sprayer_interfaces::msg::HumanCmd>::SharedPtr subscription_cmd;

    rclcpp::Subscription<basic_sprayer_interfaces::msg::Pid>::SharedPtr subscription_pid;

    rclcpp::Publisher<basic_sprayer_interfaces::msg::DebugPower>::SharedPtr debug_; // 调试
    /*创建一个定时器*/
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::TimerBase::SharedPtr timer_100ms;
    // rclcpp::SubscriptionOptions sub_adc_opt; // adc的Qos
    int adc_cnt = 0;
    int driver_cnt = 0;
    int cmd_cnt = 0;

    bool is_start;
    float speed_target;
    float angle_target; // 目标角度

    float speed_target_raw; //

    float angle_front;
    float angle_rear;
    bool e_stop = true;

    int64_t timestamp; // 时间戳

    kineControler *controler; // 控制器

    /*定时任务*/
    void timer_callback()
    {
        /*500ms的时间内没有发生通信  直接停止运行*/
        if ((cmd_cnt == 0) || (driver_cnt == 0) || (adc_cnt == 0))
        {
            e_stop = true;
            /* 发布消息让电机 转 */
            auto message = basic_sprayer_interfaces::msg::FourWheel();
            is_start = false;
            speed_target = 0;
            message.start_flag = 0;
            message.free = 1;
            message.mode = message.DRIVER_MODE_SPEED;
            message.output_left_front = 0; // 转换到rpm
            message.output_right_front = 0;
            message.output_left_rear = 0;
            message.output_right_rear = 0;
            publisher_->publish(message);
        }
        else
        {
            e_stop = false;
        }
        cmd_cnt = 0;
        driver_cnt = 0;
        adc_cnt = 0;
    }
    /*定时任务 10ms*/
    void timer_10ms_callback()
    {
        controler->setStatus(angle_front, angle_rear);
        // RCLCPP_INFO(this->get_logger(), "%d,%d", is_start, e_stop);
        /* 运行控制器 */
        if (is_start && (e_stop == false))
        {
            speed_target = speed_target_raw;
            FourValue value = controler->run(speed_target, angle_target);
            /* 增加功率平衡 */
            auto message = basic_sprayer_interfaces::msg::FourWheel();
            message.header.stamp = this->now(); // 给定时间
            message.start_flag = is_start ? 1 : 0;
            message.mode = message.DRIVER_MODE_SPEED;
            if (speed_target == 0)
            {
                message.free = 1; // 表示速度降低到0
            }
            else
            {
                message.free = 0;
            }
            message.output_left_front = value.V_fl; // 转换到rpm
            message.output_right_front = value.V_fr;
            message.output_left_rear = value.V_rl;
            message.output_right_rear = value.V_rr;
            publisher_->publish(message);
        }
        else
        {
            speed_target = 0;
            auto message = basic_sprayer_interfaces::msg::FourWheel();
            message.header.stamp = this->now(); // 给定时间
            is_start = false;
            message.start_flag = 0;
            message.mode = message.DRIVER_MODE_SPEED;
            message.output_left_front = 0; // 转换到rpm
            message.output_right_front = 0;
            message.output_left_rear = 0;
            message.output_right_rear = 0;
            publisher_->publish(message);
            controler->clear(); // 清除数据
        }
        /* 发送 debug 数据 */
        auto debug = basic_sprayer_interfaces::msg::DebugPower();
        debug.speed[0] = controler->debug_spd[0];
        debug.speed[1] = controler->debug_spd[1];
        debug.speed[2] = controler->debug_spd[2];
        debug.speed[3] = controler->debug_spd[3];

        debug.delta[0] = controler->debug_delta[0];
        debug.delta[1] = controler->debug_delta[1];
        debug.delta[2] = controler->debug_delta[2];
        debug.delta[3] = controler->debug_delta[3];

        debug_->publish(debug);
    }

    void pid_callback(const basic_sprayer_interfaces::msg::Pid::SharedPtr msg)
    {
        float p = msg->p;
        float d = msg->d;
        float i = msg->i;
        RCLCPP_INFO(this->get_logger(), "p=%f,i=%f,d=%f", p, i, d);
        // controler->set_pd(p, d);
        controler->set_power_blance_pi(p, i);
    }

    /*订阅adc*/
    void callback_adc(const basic_sprayer_interfaces::msg::Adc::SharedPtr msg)
    {
        adc_cnt++;
        angle_front = msg->angle_deg[0];
        angle_rear = msg->angle_deg[1];
    }

    /**/
    void callback_status(const basic_sprayer_interfaces::msg::FourDrivers::SharedPtr msg)
    {
        driver_cnt++;
        /* 统计功率 计算 delta_v */
        controler->set_power(msg->driver[0].power_w, msg->driver[1].power_w, msg->driver[2].power_w, msg->driver[3].power_w);
        controler->set_current(msg->driver[0].current_a, msg->driver[1].current_a, msg->driver[2].current_a, msg->driver[3].current_a);
        controler->set_speed(msg->driver[0].speed_rpm, msg->driver[1].speed_rpm, msg->driver[2].speed_rpm, msg->driver[3].speed_rpm);
    }

    void callback_cmd(const basic_sprayer_interfaces::msg::HumanCmd::SharedPtr msg)
    {
        cmd_cnt++;
        is_start = (msg->cmd_switch == msg->CMD_START);
        speed_target_raw = msg->velocity;
        // speed_target_raw = 0;
        angle_target = msg->angle;
    }
};

int main(int argc, char *argv[])
{
    /*创建 ros */
    rclcpp::init(argc, argv);
    auto pub_node = std::make_shared<kinematicsNode>();
    RCLCPP_INFO(pub_node->get_logger(), "kine has started!");
    // std::cout<<"123"<<std::endl;
    rclcpp::spin(pub_node);
    /*创建线程*/
    rclcpp::shutdown();
    return 0;
}
